namespace Microsoft.Robotics.Core.Algorithms
{
    using System;

    /// <summary>
    /// Implementation of a generic PID Controller
    /// </summary>
    public class PidController : IGenericPidController
    {
        /// <summary>
        /// Previous error value.
        /// </summary>
        private double previousError = 0;

        /// <summary>
        /// Integral component.
        /// </summary>
        private double integral = 0;

        /// <summary>
        /// Maximum output.
        /// </summary>
        private double maxOutput = 0;

        /// <summary>
        /// Minimum output.
        /// </summary>
        private double minOutput = 0;

        /// <summary>
        /// Minimum abs output.
        /// </summary>
        private double minAbsoluteOutput = 0;

        /// <summary>
        /// Last measurement timestamp.
        /// </summary>
        private long lastMeasurementTicksMicroseconds = 0;

        /// <summary>
        /// Time beyond which we don't count integral component.
        /// </summary>
        private long measurementExpiryTicks = 0;

        /// <summary>
        /// Initializes a new instance of the PidController class.
        /// </summary>
        /// <param name="maxOutput">Output will never exceed this value (clamping)</param>
        /// <param name="minOutput">Output will never be less than this value (clamping)</param>
        /// <param name="minAbsoluteOutput">
        /// Absolute value of output will never be less than this value (clamping). This is useful
        /// for cases like "Value can be -10 to -5 or 5 to 10, but never 3 or -3"
        /// </param>
        /// <param name="proportionalGain">Proportional gain parameter</param>
        /// <param name="integralGain">Integral gain parameter</param>
        /// <param name="derivativeGain">Derivative gain parameter</param>
        /// <param name="measurementExpiryTicks">
        /// Controls how quickly measurements expire, thus re-setting I and D components of
        /// the output gains.
        /// </param>
        public PidController(
            double maxOutput,
            double minOutput,
            double minAbsoluteOutput,
            double proportionalGain,
            double integralGain,
            double derivativeGain,
            long measurementExpiryTicks)
        {
            this.maxOutput = maxOutput;
            this.minOutput = minOutput;
            this.minAbsoluteOutput = minAbsoluteOutput;
            this.ProportionalGain = proportionalGain;
            this.IntegralGain = integralGain;
            this.DerivativeGain = derivativeGain;
            this.measurementExpiryTicks = measurementExpiryTicks;
        }

        /// <summary>
        /// Gets or sets proportional parameter
        /// </summary>
        public double ProportionalGain { get; set; }

        /// <summary>
        /// Gets or sets integral parameter
        /// </summary>
        public double IntegralGain { get; set; }

        /// <summary>
        /// Gets or sets derivative parameter
        /// </summary>
        public double DerivativeGain { get; set; }

        /// <summary>
        /// PID evaluation. Uses P,I,D parameters supplied at initialization time
        /// </summary>
        /// <param name="measuredError">Measured deviation from the goal</param>
        /// <param name="measurementTimeTicksMicroseconds">Measurement time in microsecond ticks</param>
        /// <returns>Suggested output</returns>
        public double Evaluate(double measuredError, long measurementTimeTicksMicroseconds)
        {
            return this.Evaluate(measuredError, measurementTimeTicksMicroseconds, this.ProportionalGain, this.IntegralGain, this.DerivativeGain);
        }
        
        /// <summary>
        /// Use to tune PID by allowing selective control over P, I, D gains directly (i.e. on controller tuning stage)
        /// </summary>
        /// <param name="measuredError">Measured deviation from the goal</param>
        /// <param name="measurementTimeTicksMicroseconds">Measurement time in microsecond ticks</param>
        /// <param name="proportionalGain">Proportional parameter</param>
        /// <param name="integralGain">Integral parameter</param>
        /// <param name="derivativeGain">Derivative parameter</param>
        /// <returns>Suggested output</returns>
        public double Evaluate(
            double measuredError, 
            long measurementTimeTicksMicroseconds, 
            double proportionalGain, 
            double integralGain, 
            double derivativeGain)
        {
            // can not use integral and derivative if at least 2 measurements have not been made, or
            // dt overflows or dt is 0 or too much time has passed since last measurement.
            double dt = measurementTimeTicksMicroseconds - this.lastMeasurementTicksMicroseconds;

            // derivative and integral will remain 0 unless there is history of prior reading
            double derivative = 0.0;

            if (0 == this.lastMeasurementTicksMicroseconds
                || 0 >= dt
                || dt > this.measurementExpiryTicks)
            {
                // no integral or derivative component for this measurement 
                this.integral = 0.0;
            }
            else 
            {
                // this is strictly speaking not necessary, but results in smaller KP, KI, KD values
                // in typical cases (i.e. no 0.00000001 or 1200000)
                double normalized = dt / (1000 * 1000);

                this.integral = this.integral + (measuredError * normalized);
                derivative = (measuredError - this.previousError) / normalized;
            }

            this.lastMeasurementTicksMicroseconds = measurementTimeTicksMicroseconds;            

            this.previousError = measuredError;

            double output = 
                -proportionalGain * measuredError -
                integralGain * this.integral -
                derivativeGain * derivative;

            return this.ClampOutput(output);
        }

        /// <summary>
        /// Clamp output to pre-set min/max and absolute min values
        /// </summary>
        /// <param name="output">Unclamped output</param>
        /// <returns>Output value.</returns>
        private double ClampOutput(double output)
        {
            // this step is necessary to clamp output to min absolute value (regardless of sign).
            // Say we want motor power be set to 1.0 to 0.2 or -1.0 to - 0.2, but never 0.19 or -0.19
            // Note, when error is 0 - the result of this will be 0 too regardless of abs. clamping values
            // use maxOutput / minOutput settings to ensure non-zero output
            if (Math.Abs(output) < this.minAbsoluteOutput)
            {
                output = Math.Sign(output) * this.minAbsoluteOutput;
            }

            // Clamp to Max/Min as per settings
            if (output > this.maxOutput)
            {
                output = this.maxOutput;
            }
            else if (output < this.minOutput)
            {
                output = this.minOutput;
            }

            return output;
        }
    }
}
